#include "Entity.h"

void Entity::calcBoundingSize()
{
	if( getShape()->getType() == Shape::SPHERE )
		return;

	// Bounding size resetten
	m_boundingsize.x = 0;
	m_boundingsize.y = 0;
	m_boundingsize.z = 0;

	//langs alle punten lopen
	int size = m_points.size();
	for(int i = 0; i < size; i++){

		//vergelijken welke de grootste waarde heeft
		if(abs(m_points[i].x - getAbsolutePosition().x) >  m_boundingsize.x)
			m_boundingsize.x = fabs(m_points[i].x - getAbsolutePosition().x);
		if(abs(m_points[i].y - getAbsolutePosition().y) >  m_boundingsize.y)
			m_boundingsize.y = fabs(m_points[i].y - getAbsolutePosition().y);
		if(abs(m_points[i].z- getAbsolutePosition().z) >  m_boundingsize.z)
			m_boundingsize.z = fabs(m_points[i].z - getAbsolutePosition().z);
	}
}

Entity::Entity() : AbstractEntity()
{

}

Entity::Entity( const Position& pos ) : AbstractEntity()
{
	setPosition(pos);
}

void Entity::preFrame( float frametime, const std::vector<Matrix4>& matrixes )
{
	AbstractEntity::preFrame(frametime, matrixes);

	m_matrixes.push_back( Transformation::getTranslationMatrix(getPosition()) );

	m_matrixes.push_back( Transformation::getTranslationMatrix(getAbsolutePosition()*-1) );
	m_matrixes.push_back( Transformation::getRotationMatrix(getRotation()) );
	m_matrixes.push_back( Transformation::getTranslationMatrix(getAbsolutePosition()) );
}

void Entity::postFrame()
{
	m_points.clear();

	// Bereken resultaat van alle matrixes
	Matrix4 m;
	for( int j = 0; j < m_matrixes.size(); j++ )
	{
		m = m*m_matrixes[j];
	}
	m_matrixes.clear();

	// Bereken absolute position
	Vector middle(0,0,0);
	Transformation::apply(m, middle);
	setAbsolutePosition( middle );

	// Overschrijf punten met punten uit shape
	m_points = getShape()->getVertexes();

	// Bereken nieuwe punten aan de hand van de matrix
	Transformation::apply(m, m_points);

	// Points have changed, recalculate bounds 
	calcBoundingSize();
}

void Entity::accept( AbstractVisitor* v )
{
	v->visit(this);
}

bool Entity::hasCollision( AbstractEntity* other, bool doublechecked )
{
	if( !doublechecked && other->hasCollision( this, true ) )
		return true;

	//Broadphase

	//x-as vergelijken
	if(this->getAbsolutePosition().x + this->getBoundingSize().x > other->getAbsolutePosition().x + other->getBoundingSize().x && 
		other->getAbsolutePosition().x + other->getBoundingSize().x > this->getAbsolutePosition().x - this->getBoundingSize().x
		||
		this->getAbsolutePosition().x + this->getBoundingSize().x > other->getAbsolutePosition().x - other->getBoundingSize().x && 
		other->getAbsolutePosition().x - other->getBoundingSize().x > this->getAbsolutePosition().x - this->getBoundingSize().x)
	{}else{return false; }

	//y-as vergelijken
	if(this->getAbsolutePosition().y + this->getBoundingSize().y >= other->getAbsolutePosition().y + other->getBoundingSize().y && 
		other->getAbsolutePosition().y + other->getBoundingSize().y >= this->getAbsolutePosition().y - this->getBoundingSize().y
		||
		this->getAbsolutePosition().y + this->getBoundingSize().y >= other->getAbsolutePosition().y - other->getBoundingSize().y && 
		other->getAbsolutePosition().y - other->getBoundingSize().y >= this->getAbsolutePosition().y - this->getBoundingSize().y)
	{}else{return false; }

	//z-as vergelijken
	if(this->getAbsolutePosition().z + this->getBoundingSize().z > other->getAbsolutePosition().z + other->getBoundingSize().z && 
		other->getAbsolutePosition().z + other->getBoundingSize().z > this->getAbsolutePosition().z - this->getBoundingSize().z
		||
		this->getAbsolutePosition().z + this->getBoundingSize().z > other->getAbsolutePosition().z - other->getBoundingSize().z && 
		other->getAbsolutePosition().z - other->getBoundingSize().z > this->getAbsolutePosition().z - this->getBoundingSize().z)
	{}else{return false; }

	const Position& otherPos = other->getAbsolutePosition();
	const Dimensions& otherBounding = other->getBoundingSize();

	// Narrowphase
	if( !other->isGroup() )
	{
		Entity* otherEntity = static_cast<Entity*>(other);
		Shape* otherShape = otherEntity->getShape();

		// Faces van this controleren
		for( int i = 0; i < getShape()->getFaces().size(); i++ )
		{
			float max0, min0, max1, min1;
			Vector D = getAbsolutePosition() + getShape()->getFaces()[i];
			berekenMaxMin(this, D, min0, max0);
			berekenMaxMin(otherEntity, D, min1, max1);

			if( max1 < min0 || max0 < min1 )
			{
				return false;
			}
		}

		// Faces van other controleren
		for( int i = 0; i < otherEntity->getShape()->getFaces().size(); i++ )
		{
			float max0, min0, max1, min1;
			Vector D = otherEntity->getAbsolutePosition() + otherEntity->getShape()->getFaces()[i];
			berekenMaxMin(this, D, min0, max0);
			berekenMaxMin(otherEntity, D, min1, max1);

			if( max1 < min0 || max0 < min1 )
			{
				return false;
			}
		}

		// Edges vergelijken
		for( int i = 0; i < getShape()->getEdges().size(); i++ )
		{
			for( int j = 0; j < otherEntity->getShape()->getEdges().size(); j++ )
			{
				float max0, min0, max1, min1;
				Vector D = getShape()->getEdges()[i].uitProduct( otherEntity->getShape()->getEdges()[j] );
				berekenMaxMin(this, D, min0, max0);
				berekenMaxMin(otherEntity, D, min1, max1);

				if( max1 < min0 || max0 < min1 )
				{
					return false;
				}
			}
		}
		return true;
	}

	return false;
}

void Entity::berekenMaxMin( Entity* c, Vector D, float& min, float& max )
{
	min = D.dotProduct( c->getAbsolutePosition() + c->getShape()->getVertexes()[0] );
	max = min;

	for( int i = 1; i < c->getShape()->getVertexes().size(); i++ )
	{
		float value = D.dotProduct( c->getAbsolutePosition() + c->getShape()->getVertexes()[i] );

		if( value < min )
			min = value;
		else if( value > max )
			max = value;
	}
}

void Entity::checkCollision( AbstractEntity* other )
{
	if( other->isGroup() )
	{
		std::vector<AbstractEntity*>::const_iterator it, it2;
		for( it = other->getChildren().begin(); it != other->getChildren().end(); it++ )
		{
			checkCollision( *it );
		}
	}
	else
	{
		if( hasCollision( other ) || other->hasCollision( this ) )
		{
			collisionResponse(other);
			other->collisionResponse(this);
		}
	}
}